package routmap

import (
	"errors"
	"math"
	"strconv"

	"github.com/emirpasic/gods/maps/hashmap"
	"github.com/emirpasic/gods/maps/treemap"
)

var (
	open_map  *treemap.Map
	close_map *treemap.Map
)

var (
	Can_use     = 1
	Can_not_use = -1
)

type Point struct {
	x, y, z    int
	state      *int
	f          int //权重值
	next_nodes *treemap.Map
}
type RoutingMap struct {
	width, height int
	startPoint    *Point
	endPoint      *Point
	point_list    *hashmap.Map
}

func CreateMap(width, height int) *RoutingMap {
	my_map := RoutingMap{width: width, height: height}

	my_map.point_list = hashmap.New()

	for x := 0; x < width; x++ {
		for y := 0; y < height; y++ {
			point := Point{x: x, y: y, z: 0, state: &Can_use}
			my_map.point_list.Put(strconv.Itoa(x)+strconv.Itoa(y), &point)
		}
	}

	return &my_map
}
func (routingMap *RoutingMap) SetPoint(x, y int, state int) {
	i, _ := routingMap.point_list.Get(strconv.Itoa(x) + strconv.Itoa(y))

	if values, ok := i.(*Point); ok {
		values.state = &state
	}
}
func (routingMap *RoutingMap) SetStartPoint(x, y int) {
	point := Point{x: x, y: y}

	routingMap.startPoint = &point
}
func (routingMap *RoutingMap) SetEndPoint(x, y int) {
	point := Point{x: x, y: y}

	routingMap.endPoint = &point
}

/**
进行导航
*/
func (routingMap *RoutingMap) Routing() (int, error) {
	if routingMap.startPoint == nil || routingMap.endPoint == nil {
		return -1, errors.New("请先设置开始和结束点")
	}

	//初始化开放表和关闭表
	open_map = treemap.NewWith(PointfComparator)
	close_map = treemap.NewWith(PointfComparator)

	open_map.Put(routingMap.startPoint, routingMap.startPoint)

	for !hasRoutingEnd(routingMap) {
		_, v := open_map.Min()

		if center_point, ok := v.(*Point); ok {
			//得到f最小值开放点周围的八个点
			var minX, minY, maxX, maxY int
			if center_point.x-1 >= 0 {
				minX = center_point.x - 1
			} else {
				minX = center_point.x
			}
			if center_point.y-1 >= 0 {
				minY = center_point.y - 1
			} else {
				minY = center_point.y
			}
			if center_point.x+1 <= routingMap.width {
				maxX = center_point.x + 1
			} else {
				maxX = center_point.x
			}
			if center_point.y+1 <= routingMap.height {
				maxY = center_point.y + 1
			} else {
				maxY = center_point.y
			}
			if center_point.next_nodes == nil {
				center_point.next_nodes = treemap.NewWith(PointfComparator)
			}

			for x := minX; x <= maxX; x++ {
				for y := minY; y <= maxY; y++ {
					i, _ := routingMap.point_list.Get(strconv.Itoa(x) + strconv.Itoa(y))

					if round_point, ok := i.(*Point); ok && !isInCloseMap(round_point) &&
						!isInOpenMap(round_point) && !isBlock(round_point, routingMap) {

						round_point.f = matchGn(center_point, round_point) + matchHn(round_point, routingMap.endPoint)
						center_point.next_nodes.Put(round_point, round_point)
						open_map.Put(round_point, round_point)
					}
				}
			}

			open_map.Remove(center_point)
			close_map.Put(center_point, center_point)
		}
	}

	return 0, nil
}
func isInOpenMap(point *Point) bool {
	iterator := open_map.Iterator()

	for iterator.Next() {
		if values, ok := iterator.Value().(*Point); ok && values == point {
			return true
		}
	}

	return false
}
func isInCloseMap(point *Point) bool {
	iterator := close_map.Iterator()

	for iterator.Next() {
		if values, ok := iterator.Value().(*Point); ok && values == point {
			return true
		}
	}

	return false
}
func isBlock(point *Point, routingMap *RoutingMap) bool {
	i, _ := routingMap.point_list.Get(strconv.Itoa(point.x) + strconv.Itoa(point.y))

	if values, ok := i.(Point); ok {
		return *values.state == Can_not_use
	}

	return false
}

/*
计算移动损耗
*/
func matchGn(parentPoint *Point, nowPoint *Point) int {
	g := math.Sqrt(math.Pow(float64(parentPoint.x)-float64(nowPoint.x), 2) - math.Pow(float64(parentPoint.y)-float64(nowPoint.y), 2))

	return int(g * 10)
}

/*
计算当前点到终点的损耗
*/
func matchHn(nowPoint *Point, endPoint *Point) int {
	h := math.Sqrt(math.Pow(float64(endPoint.x)-float64(nowPoint.x), 2) - math.Pow(float64(endPoint.y)-float64(nowPoint.y), 2))

	return int(h * 10)
}
func checkIsClosed() bool {

	return false
}

/*
寻路是否结束
*/
func hasRoutingEnd(routingMap *RoutingMap) bool {
	iterator := open_map.Iterator()

	for iterator.Next() {
		if values, ok := iterator.Value().(*Point); ok && values == routingMap.endPoint {
			return true
		}
	}

	return false
}

func PointfComparator(a, b interface{}) int {
	point_a, _ := a.(*Point)
	point_b, _ := b.(*Point)

	return point_a.f - point_b.f
}
